/*
 * EKFKinematic.cpp
 *
 *  Created on: May 26, 2011
 *      Author: gbower
 */

#include "EKFKinematic.h"
#include <iostream>
using namespace std;

const int ione = 1;
const double done = 1.0;
const double dzero = 0;


/**********************************
Initialize the filter
**********************************/
EKFKinematic::EKFKinematic(Aircraft* aci) {

// Test matrix for debugging...
//	for (int i = 0; i < numStates; i++) {
//		for (int j = 0; j < numStates;j++) {
//			if (i == j) {
//				P[i][j] = 1;
//			} else if (j > i){
//				P[i][j] = (double)(i+1)/(double)(j+1);
//			} else if (i > j) {
//				P[i][j] = (double)(j+1)/(double)(i+1);
//			}
//		}
//	}


	// Model States: 
	// Inertial positions: N, E, D
	// Inertial velocities: VN, VE, VD
	// Attitude quaternions: q0, q1, q2, q3
	// North and East Winds: WindN, WindE
	// Gyro biases: wbx, wby, wbz
	// Pressure biases: barob, diffb
	// Sensor offset angles: psis, thetas, phis
	
	
	// Initialize Covariance Matrix
	double P_Diag_Init = {100.0,100.0,100.0,10.0,10.0,10.0,0.01,0.01,0.01,0.01,4.0,4.0,0.001,0.001,0.001,100.0,10.0,0.001,0.001,0.001};

	for (int i = 0; i<numStates; i++) {
		for (int j = 0; j<numStates; j++) {
			if (i == j) {
				*(P+i+j*numStates) = P_Diag_Init[i];
			} else {
				*(P+i+j*numStates) = 0.0;
			}
		}
	}
	
	// Process noise covariance matrix (assumed diagonal)
	R = {0.1,0.1,0.1,0.1,0.1,0.1,1e-4,1e-4,1e-4,1e-4,1e-4,1e-4,1e-6,1e-6,1e-6,1e-2,1e-4,1e-8,1e-8,1e-8};
	
	// Measurement covariance matrices (assumed diagonal)	
	// Baro pressure, Differential pressure, Mx, My, Mz, GPSN, GPSE, GPSD, GPSVN, GPSVE
	Q_IMU_GPS = {100.0,50.0,0.001,0.001,0.001,10.0,10.0,25.0,0.1,0.1};
	Q_IMU = {100.0,50.0,0.001,0.001,0.001};
	
	// Nominal filter timestep
	dt = 0.01;

	// Initialize state vector
	pv = new paramVector;
	sv = new stateVector;

	// Open file to record data
	Sfile = new ofstream ("stateout.txt");

	// Clear Jacobian matrices
	for (int i = 0; i < numStates*numStates; i++) {
		*(Gs+i) = 0;
	}
	for (int i = 0; i < numStates*(imuSize+gpsSize); i++) {
		*(Hs+i) = 0;
	}
}


/**********************************
What to do when data is received
from the autopilot. Run an
iteration of the fitler!
**********************************/
void EKFKinematic::AUPdataReceived(Communication &com) {
	
	// Determine packet type
	bool flag = true;

	if (com.datInitReceived && !com.runEKF) {
		initializeState();
		com.runEKF = true;
		dt = 0.01;
		ts = com.timer.getTime();
		cout << "Initial GPS packet received.." << endl;
	} else if (com.runEKF && com.imuin) {
		dt = com.tIMU[1] - com.tIMU[0];
		
		/*
		 *   Nothing happens when a GPS packet is received. We always wait for the
		 *   next IMU packet before performing an update. So check to see if the
		 *   latest GPS time is between the current IMU time and the previous IMU
		 *   time. If so, perform the combined measurement update (flag = false).
		 */
		if ((com.tIMU[1] > com.tGPS[1]) && (com.tIMU[0] < com.tGPS[1])) {
			flag = false;
		}

		// Initialize local variables
		int N = numStates;
		int M;
		double *xcurr = (double*)malloc(sizeof(double)*numStates);
		double *mubt = (double*)malloc(sizeof(double)*numStates);
		double *G = (double*)malloc(sizeof(double)*numStates*numStates);
		double *Sbart = (double*)malloc(sizeof(double)*numStates*numStates);
		double *H, *Kt, *zbt;
		if (flag) {
			M = imuSize;
			H  = (double*)malloc(sizeof(double)*N*M);
			Kt = (double*)malloc(sizeof(double)*N*M);
			zbt = (double*)malloc(sizeof(double)*M);
		} else {
			M = imuSize+gpsSize;
			H = (double*)malloc(sizeof(double)*N*M);
			Kt = (double*)malloc(sizeof(double)*N*M);
			zbt = (double*)malloc(sizeof(double)*M);
		}

		// 1. Get current state, get mu
		getCurrentState(xcurr);

		// 2. Propogate the state, compute mubt
		runProcessModel(xcurr, mubt, dt);
		
		// 3. Compute process Jacobian, compute G
		computeProcessJacobian(xcurr, G, dt);
		
		// 4. Propogate the covariance, compute Sbart
					computePredictedVariance(G, SBart); // NEED TO CHECK/UPDATE!

		// 5. Compute the expected measurements, compute zbt
		runMeasModel(mubt, zbt, flag);
		
		// 6. Compute the measurement Jacobian, compute H
		computeMeasJacobian(mubt, H, flag);
		
		// 7. Calculate the Kalman gain, compute K
					computeKalmanGain(Sbart, H, Kt, flag);	// NEED TO CHECK
				
		// 8. Update the state, compute mu
		// 9. Update the covariance, compute Sigma
					computeNewMeanandCovariance(mubt, Kt, zbt, Sbart, H, flag);	// NEED TO CHECK

		// Print state
		printData(com.tIMU[1]);		
		
//		cout <<  *(xcurr+0) << " " << *(xcurr+1) << " " << *(xcurr+2) << " " <<
//				 *(xcurr+3) << " " <<  *(xcurr+4) << " " <<  *(xcurr+5) << " " <<
//				 *(xcurr+6) << " " <<  *(xcurr+7) << " " <<  *(xcurr+8) << " " <<
//				 *(xcurr+9) << " " <<  *(xcurr+10) << " " <<  *(xcurr+11) << " " <<
//				 *(xcurr+12) << " " <<  *(xcurr+13) << " " <<  *(xcurr+14) << " " << endl;
		
		free(xcurr); free(G); free(mubt); free(Sbart);
		free(Kt); free(H); free(zbt);
	}
}


/**********************************
Initialize the current state
**********************************/
void EKFKinematic::initializeState() {
	// Initialize state
	sv->Np  = apdat.GPS_N;
	sv->Ep  = apdat.GPS_E;
	sv->Dp  = apdat.GPS_D;
	sv->VNi = 0.0; //apdat.GPS_VN;
	sv->VEi = 0.0; //apdat.GPS_VE;
	sv->VDi = 0.0; //0.0;
	sv->q0  = 1.0;
	sv->q1  = 0.0;
	sv->q2  = 0.0;
	sv->q3  = 0.0;
	pv->VNi_wind = 0.0;
	pv->VEi_wind = 0.0;
	pv->wbx = apdat.p;
	pv->wby = apdat.q;
	pv->wbz = apdat.r;
	pv->barob = -apdat.baropress + 101325.0*pow((1.0+0.0000225721785*apdat.GPS_D),5.256);
	pv->diffb = 0.0;
	pv->psis = 0.0;
	pv->thetas = 0.0;
	pv->phis = 0.0;
}



/**********************************
Get the current state
**********************************/
void EKFKinematic::getCurrentState(double *xcurr) {
	*xcurr      = sv->Np;
	*(xcurr+1)  = sv->Ep;
	*(xcurr+2)  = sv->Dp;
	*(xcurr+3)  = sv->VNi;
	*(xcurr+4)  = sv->VEi;
	*(xcurr+5)  = sv->VDi;
	*(xcurr+6)  = sv->q0;
	*(xcurr+7)  = sv->q1;
	*(xcurr+8)  = sv->q2;
	*(xcurr+9)  = sv->q3;
	*(xcurr+10) = pv->VNi_wind;
	*(xcurr+11) = pv->VEi_wind;
	*(xcurr+12) = pv->wbx;
	*(xcurr+13) = pv->wby;
	*(xcurr+14) = pv->wbz;
	*(xcurr+15) = pv->barob;
	*(xcurr+16) = pv->diffb;
	*(xcurr+17) = pv->psis;
	*(xcurr+18) = pv->thetas;
	*(xcurr+19) = pv->phis;
}


/**********************************
Run the kinematic process model
**********************************/
void EKFKinematic::runProcessModel(double *x, double *mubt, double dt) {

	// Extract state
	double Np = *(x+0);
	double Ep = *(x+1);
	double Dp = *(x+2);
	double VNi = *(x+3);
	double VEi = *(x+4);
	double VDi = *(x+5);
	double q0 = *(x+6);
	double q1 = *(x+7);
	double q2 = *(x+8);
	double q3 = *(x+9);
	double VNw = *(x+10);
	double VEw = *(x+11);
	double wbx = *(x+12);
	double wby = *(x+13);
	double wbz = *(x+14);
	double barob = *(x+15);
	double diffb = *(x+16);
	double psis = *(x+17);
	double thetas = *(x+18);
	double phis = *(x+19);
	
	// Extract measurements
	double ax = apdat.ax1;
	double ay = apdat.ay1;
	double az = apdat.az1;
	double wx = apdat.p;
	double wy = apdat.q;
	double wz = apdat.r;

	// Constants
	double g = 9.81; // Acceleration due to gravity [m/s^2]

	double cpsis = cos(psis);
	double spsis = sin(psis);
	double cthetas = cos(thetas);
	double sthetas = sin(thetas);
	double cphis = cos(phis);
	double sphis = sin(phis);

	// Position propogation
	*(mubt+0) = Np + VNi*dt;
	*(mubt+1) = Ep + VEi*dt;
	*(mubt+2) = Dp + VDi*dt;

	// Velocity propogation
	double Ts2b[9]; 	// Rotation matrix from sensor frame to body frame
	double Tb2i[9];		// Rotation matrix from body frame to inertial frame
	// In the form [ 0 1 2 ; 3 4 5 ; 6 7 8];
	
	Ts2b[0] = cthetas*cpsis;
	Ts2b[1] = sphis*sthetas*cpsis-cphis*spsis;
	Ts2b[2] = cphis*sthetas*cpsis+sphis*spsis;
	Ts2b[3] = cthetas*spsis;
	Ts2b[4] = sphis*sthetas*spsis+cphis*cpsis;
	Ts2b[5] = cphis*sthetas*spsis-sphis*cpsis;
	Ts2b[6] = -sthetas;
	Ts2b[7] = sphis*cthetas;
	Ts2b[8] = cphis*cthetas;
	
	Tb2i[0] = 1.0-2.0*q2*q2-2.0*q3*q3;
	Tb2i[1] = 2.0*q1*q2-2.0*q0*q3;
	Tb2i[2] = 2.0*q1*q3+2.0*q0*q2;
	Tb2i[3] = 2.0*q1*q2+2.0*q0*q3;
	Tb2i[4] = 1.0-2.0*q1*q1-2.0*q3*q3;
	Tb2i[5] = 2.0*q2*q3-2.0*q0*q1;
	Tb2i[6] = 2.0*q1*q3-2.0*q0*q2;
	Tb2i[7] = 2.0*q2*q3+2.0*q0*q1;
	Tb2i[8] = 1.0-2.0*q1*q1-2.0*q2*q2;

	double Ts2b_times_acc[3];
	Ts2b_times_acc[0] = Ts2b[0]*ax + Ts2b[1]*ay + Ts2b[2]*az;
	Ts2b_times_acc[1] = Ts2b[3]*ax + Ts2b[4]*ay + Ts2b[5]*az;
	Ts2b_times_acc[2] = Ts2b[6]*ax + Ts2b[7]*ay + Ts2b[8]*az;

	*(mubt+3) = VNi + (Tb2i[0]*Ts2b_times_acc[0] + Tb2i[1]*Ts2b_times_acc[1] + Tb2i[2]*Ts2b_times_acc[2])*dt;
	*(mubt+4) = VEi + (Tb2i[3]*Ts2b_times_acc[0] + Tb2i[4]*Ts2b_times_acc[1] + Tb2i[5]*Ts2b_times_acc[2])*dt;
	*(mubt+5) = VDi + (Tb2i[6]*Ts2b_times_acc[0] + Tb2i[7]*Ts2b_times_acc[1] + Tb2i[8]*Ts2b_times_acc[2])*dt - g*dt;

	// Calculate delta euler angles taking sensor misalignment into account
	double dPhi   = (Ts2b[0]*(wx-wbx) + Ts2b[1]*(wy-wby) + Ts2b[2]*(wz-wbz))*dt;
	double dTheta = (Ts2b[3]*(wx-wbx) + Ts2b[4]*(wy-wby) + Ts2b[5]*(wz-wbz))*dt;
	double dPsi   = (Ts2b[6]*(wx-wbx) + Ts2b[7]*(wy-wby) + Ts2b[8]*(wz-wbz))*dt;
	double s = 0.5*sqrt(dPhi*dPhi + dTheta*dTheta + dPsi*dPsi);
	double coss = cos(s);
	double sins_over_s;
	if (s == 0) {
		sins_over_s = 1.0;
	} else {
		sins_over_s = sin(s)/s;
	}
	
	// Calculate new quaternions
	double qnew[4];
	qnew[0] = coss*q0 - 0.5*sins_over_s*(dPhi*q1 + dTheta*q2 + dPsi*q3);
	qnew[1] = coss*q1 - 0.5*sins_over_s*(-dPhi*q0 - dPsi*q2 + dtheta*q3);
	qnew[2] = coss*q2 - 0.5*sins_over_s*(-dTheta*q0 + dPsi*q1 - dPhi*q3);
	qnew[3] = coss*q3 - 0.5*sins_over_s*(-dPsi*q0 - dTheta*q1 + dPhi*q2);

	// Normalize quaternions
	double invqmag = 1.0/sqrt(qnew[0]*qnew[0] + qnew[1]*qnew[1] + qnew[2]*qnew[2] + qnew[3]*qnew[3]);
	*(mubt+6) = qnew[0]*invqmag;
	*(mubt+7) = qnew[1]*invqmag;
	*(mubt+8) = qnew[2]*invqmag;
	*(mubt+9) = qnew[3]*invqmag;

	// Parameter estimates don't change
	*(mubt+10) = VNw;
	*(mubt+11) = VEw;
	*(mubt+12) = wbx;
	*(mubt+13) = wby;
	*(mubt+14) = wbz;
	*(mubt+15) = barob;
	*(mubt+16) = diffb;
	*(mubt+17) = psis;
	*(mubt+18) = thetas;
	*(mubt+19) = phis;
}


/**********************************
Compute process model Jacobian
**********************************/
void EKFKinematic::computeProcessJacobian(double *x, double *G, double dt) {

	// i = function
	// j = state
	
	// G is in FORTRAN row major format
	// G(i,j) = *(G+j+i*numStates)
	
	// Initialize G to zeros
	for (int i = 0; i < numStates; i++) {
		for (int j = 0; j < numStates; j++) {
			*(G+j+i*numStates) = 0.0;
		}
	}

	// Extract state
	double Np = *(x+0);
	double Ep = *(x+1);
	double Dp = *(x+2);
	double VNi = *(x+3);
	double VEi = *(x+4);
	double VDi = *(x+5);
	double q0 = *(x+6);
	double q1 = *(x+7);
	double q2 = *(x+8);
	double q3 = *(x+9);
	double VNw = *(x+10);
	double VEw = *(x+11);
	double wbx = *(x+12);
	double wby = *(x+13);
	double wbz = *(x+14);
	double barob = *(x+15);
	double diffb = *(x+16);
	double psis = *(x+17);
	double thetas = *(x+18);
	double phis = *(x+19);
	
	// Extract measurements
	double ax = apdat.ax1;
	double ay = apdat.ay1;
	double az = apdat.az1;
	double wx = apdat.p;
	double wy = apdat.q;
	double wz = apdat.r;
	
	double cpsis = cos(psis);
	double spsis = sin(psis);
	double cthetas = cos(thetas);
	double sthetas = sin(thetas);
	double cphis = cos(phis);
	double sphis = sin(phis);

	// Position jacobian elements
	*(G+0+0*numStates) = 1.0; 	// dN/dN
	*(G+3+0*numStates) = dt;	// dN/dVN
	
	*(G+1+1*numStates) = 1.0;	// dE/dE
	*(G+4+1*numStates) = dt;	// dE/dVE
	
	*(G+2+2*numStates) = 1.0;	// dD/dD
	*(G+5+2*numStates) = dt;	// dD/dVD
		
	// Velocity Jacobian elements
	*(G+3+3*numStates) = 1.0;	// dVN/dVN
	*(G+6+3*numStates) = -2*dt*(q3*cthetas*spsis*ax+q3*ay*sphis*sthetas*spsis+q3*ay*cphis*cpsis+q3*az*cphis*sthetas*spsis-q3*az*sphis*cpsis+q2*sthetas*ax-q2*sphis*cthetas*ay-q2*cphis*cthetas*az);	// dVN/dq0
	*(G+7+3*numStates) = -2*dt*(q3*cthetas*spsis*ax+q3*ay*sphis*sthetas*spsis+q3*ay*cphis*cpsis+q3*az*cphis*sthetas*spsis-q3*az*sphis*cpsis+q2*sthetas*ax-q2*sphis*cthetas*ay-q2*cphis*cthetas*az);	// dVN/dq1
	*(G+8+3*numStates) = -2*dt*(2*q2*cthetas*cpsis*ax+2*q2*ay*sphis*sthetas*cpsis-2*q2*ay*cphis*spsis+2*q2*az*cphis*sthetas*cpsis+2*q2*az*sphis*spsis-q1*cthetas*spsis*ax-q1*ay*sphis*sthetas*spsis-q1*ay*cphis*cpsis-q1*az*cphis*sthetas*spsis+q1*az*sphis*cpsis+q0*sthetas*ax-q0*sphis*cthetas*ay-q0*cphis*cthetas*az);	// dVN/dq2
	*(G+9+3*numStates) = -2*dt*(2*q3*cthetas*cpsis*ax+2*q3*ay*sphis*sthetas*cpsis-2*q3*ay*cphis*spsis+2*q3*az*cphis*sthetas*cpsis+2*q3*az*sphis*spsis+q0*cthetas*spsis*ax+q0*ay*sphis*sthetas*spsis+q0*ay*cphis*cpsis+q0*az*cphis*sthetas*spsis-q0*az*sphis*cpsis+q1*sthetas*ax-q1*sphis*cthetas*ay-q1*cphis*cthetas*az);	// dVN/dq3
	*(G+17+3*numStates) = dt*(2*q2*q2*cthetas*spsis*ax+2*q2*q2*ay*cphis*cpsis-2*q2*q2*az*sphis*cpsis+2*q3*q3*cthetas*spsis*ax+2*q3*q3*ay*cphis*cpsis-2*q3*q3*az*sphis*cpsis-ay*sphis*sthetas*spsis-az*cphis*sthetas*spsis+2*q2*q2*ay*sphis*sthetas*spsis+2*q2*q2*az*cphis*sthetas*spsis+2*q3*q3*ay*sphis*sthetas*spsis+2*q3*q3*az*cphis*sthetas*spsis+2*q1*q2*cthetas*cpsis*ax+2*q1*q2*ay*sphis*sthetas*cpsis-2*q1*q2*ay*cphis*spsis+2*q1*q2*az*cphis*sthetas*cpsis+2*q1*q2*az*sphis*spsis-2*q0*q3*cthetas*cpsis*ax-2*q0*q3*ay*sphis*sthetas*cpsis+2*q0*q3*ay*cphis*spsis-2*q0*q3*az*cphis*sthetas*cpsis-2*q0*q3*az*sphis*spsis+az*sphis*cpsis-cthetas*spsis*ax-ay*cphis*cpsis);	// dVN/dpsis
	*(G+18+3*numStates) = dt*(-sthetas*cpsis*ax+sphis*cthetas*cpsis*ay+cphis*cthetas*cpsis*az+2*q2*q2*sthetas*cpsis*ax-2*q2*q2*sphis*cthetas*cpsis*ay-2*q2*q2*cphis*cthetas*cpsis*az+2*q3*q3*sthetas*cpsis*ax-2*q3*q3*sphis*cthetas*cpsis*ay-2*q3*q3*cphis*cthetas*cpsis*az-2*q1*q2*sthetas*spsis*ax+2*q1*q2*sphis*cthetas*spsis*ay+2*q1*q2*cphis*cthetas*spsis*az+2*q0*q3*sthetas*spsis*ax-2*q0*q3*sphis*cthetas*spsis*ay-2*q0*q3*cphis*cthetas*spsis*az-2*q1*q3*cthetas*ax-2*q1*q3*sphis*sthetas*ay-2*q1*q3*cphis*sthetas*az-2*q0*q2*cthetas*ax-2*q0*q2*sphis*sthetas*ay-2*q0*q2*cphis*sthetas*az);	// dVN/dthetas
	*(G+19+3*numStates) = -dt*(-ay*cphis*sthetas*cpsis-ay*sphis*spsis+az*sphis*sthetas*cpsis-az*cphis*spsis+2*q2*q2*ay*cphis*sthetas*cpsis+2*q2*q2*ay*sphis*spsis-2*q2*q2*az*sphis*sthetas*cpsis+2*q2*q2*az*cphis*spsis+2*q3*q3*ay*cphis*sthetas*cpsis+2*q3*q3*ay*sphis*spsis-2*q3*q3*az*sphis*sthetas*cpsis+2*q3*q3*az*cphis*spsis-2*q1*q2*ay*cphis*sthetas*spsis+2*q1*q2*ay*sphis*cpsis+2*q1*q2*az*sphis*sthetas*spsis+2*q1*q2*az*cphis*cpsis+2*q0*q3*ay*cphis*sthetas*spsis-2*q0*q3*ay*sphis*cpsis-2*q0*q3*az*sphis*sthetas*spsis-2*q0*q3*az*cphis*cpsis-2*q1*q3*cphis*cthetas*ay+2*q1*q3*sphis*cthetas*az-2*q0*q2*cphis*cthetas*ay+2*q0*q2*sphis*cthetas*az);	// dVN/dphis

	*(G+4+4*numStates) = 1.0;	// dVE/dVE
	*(G+6+4*numStates) = 2*dt*(q3*cthetas*cpsis*ax+q3*ay*sphis*sthetas*cpsis-q3*ay*cphis*spsis+q3*az*cphis*sthetas*cpsis+q3*az*sphis*spsis+q1*sthetas*ax-q1*sphis*cthetas*ay-q1*cphis*cthetas*az);	// dVE/dq0
	*(G+7+4*numStates) = 2*dt*(q2*cthetas*cpsis*ax+q2*ay*sphis*sthetas*cpsis-q2*ay*cphis*spsis+q2*az*cphis*sthetas*cpsis+q2*az*sphis*spsis-2*q1*cthetas*spsis*ax-2*q1*ay*sphis*sthetas*spsis-2*q1*ay*cphis*cpsis-2*q1*az*cphis*sthetas*spsis+2*q1*az*sphis*cpsis+q0*sthetas*ax-q0*sphis*cthetas*ay-q0*cphis*cthetas*az);	// dVE/dq1
	*(G+8+4*numStates) = -2*dt*(-q1*cthetas*cpsis*ax-q1*ay*sphis*sthetas*cpsis+q1*ay*cphis*spsis-q1*az*cphis*sthetas*cpsis-q1*az*sphis*spsis+q3*sthetas*ax-q3*sphis*cthetas*ay-q3*cphis*cthetas*az);	// dVE/dq2
	*(G+9+4*numStates) = -2*dt*(-q0*cthetas*cpsis*ax-q0*ay*sphis*sthetas*cpsis+q0*ay*cphis*spsis-q0*az*cphis*sthetas*cpsis-q0*az*sphis*spsis+2*q3*cthetas*spsis*ax+2*q3*ay*sphis*sthetas*spsis+2*q3*ay*cphis*cpsis+2*q3*az*cphis*sthetas*spsis-2*q3*az*sphis*cpsis+q2*sthetas*ax-q2*sphis*cthetas*ay-q2*cphis*cthetas*az);	// dVE/dq3
	*(G+17+4*numStates) = -dt*(ay*cphis*spsis+2*q1*q1*cthetas*cpsis*ax-2*q1*q1*ay*cphis*spsis+2*q1*q1*az*sphis*spsis+2*q3*q3*cthetas*cpsis*ax-2*q3*q3*ay*cphis*spsis+2*q3*q3*az*sphis*spsis-ay*sphis*sthetas*cpsis-az*cphis*sthetas*cpsis-az*sphis*spsis-cthetas*cpsis*ax+2*q1*q2*cthetas*spsis*ax+2*q1*q1*ay*sphis*sthetas*cpsis+2*q1*q1*az*cphis*sthetas*cpsis+2*q1*q2*ay*sphis*sthetas*spsis+2*q1*q2*ay*cphis*cpsis+2*q1*q2*az*cphis*sthetas*spsis-2*q1*q2*az*sphis*cpsis+2*q0*q3*cthetas*spsis*ax+2*q0*q3*ay*sphis*sthetas*spsis+2*q0*q3*ay*cphis*cpsis+2*q0*q3*az*cphis*sthetas*spsis-2*q0*q3*az*sphis*cpsis+2*q3*q3*ay*sphis*sthetas*cpsis+2*q3*q3*az*cphis*sthetas*cpsis);	// dVE/dpsis
	*(G+18+4*numStates) = -dt*(2*q1*q2*sthetas*cpsis*ax-2*q1*q2*sphis*cthetas*cpsis*ay-2*q1*q2*cphis*cthetas*cpsis*az+2*q0*q3*sthetas*cpsis*ax-2*q0*q3*sphis*cthetas*cpsis*ay-2*q0*q3*cphis*cthetas*cpsis*az+sthetas*spsis*ax-sphis*cthetas*spsis*ay-cphis*cthetas*spsis*az-2*q1*q1*sthetas*spsis*ax+2*q1*q1*sphis*cthetas*spsis*ay+2*q1*q1*cphis*cthetas*spsis*az-2*q3*q3*sthetas*spsis*ax+2*q3*q3*sphis*cthetas*spsis*ay+2*q3*q3*cphis*cthetas*spsis*az+2*q2*q3*cthetas*ax+2*q2*q3*sphis*sthetas*ay+2*q2*q3*cphis*sthetas*az-2*q0*q1*cthetas*ax-2*q0*q1*sphis*sthetas*ay-2*q0*q1*cphis*sthetas*az);	// dVE/dthetas
	*(G+19+4*numStates) = dt*(2*q1*q2*ay*cphis*sthetas*cpsis+2*q1*q2*ay*sphis*spsis-2*q1*q2*az*sphis*sthetas*cpsis+2*q1*q2*az*cphis*spsis+2*q0*q3*ay*cphis*sthetas*cpsis+2*q0*q3*ay*sphis*spsis-2*q0*q3*az*sphis*sthetas*cpsis+2*q0*q3*az*cphis*spsis+ay*cphis*sthetas*spsis-ay*sphis*cpsis-az*sphis*sthetas*spsis-az*cphis*cpsis-2*q1*q1*ay*cphis*sthetas*spsis+2*q1*q1*ay*sphis*cpsis+2*q1*q1*az*sphis*sthetas*spsis+2*q1*q1*az*cphis*cpsis-2*q3*q3*ay*cphis*sthetas*spsis+2*q3*q3*ay*sphis*cpsis+2*q3*q3*az*sphis*sthetas*spsis+2*q3*q3*az*cphis*cpsis+2*q2*q3*cphis*cthetas*ay-2*q2*q3*sphis*cthetas*az-2*q0*q1*cphis*cthetas*ay+2*q0*q1*sphis*cthetas*az);	// dVE/dphis
	
	*(G+5+5*numStates) = 1.0;	// dVD/dVD
	*(G+6+5*numStates) = -2*dt*(q2*cthetas*cpsis*ax+q2*ay*sphis*sthetas*cpsis-q2*ay*cphis*spsis+q2*az*cphis*sthetas*cpsis+q2*az*sphis*spsis-q1*cthetas*spsis*ax-q1*ay*sphis*sthetas*spsis-q1*ay*cphis*cpsis-q1*az*cphis*sthetas*spsis+q1*az*sphis*cpsis);	// dVD/dq0
	*(G+7+5*numStates) = 2*dt*(q3*cthetas*cpsis*ax+q3*ay*sphis*sthetas*cpsis-q3*ay*cphis*spsis+q3*az*cphis*sthetas*cpsis+q3*az*sphis*spsis+q0*cthetas*spsis*ax+q0*ay*sphis*sthetas*spsis+q0*ay*cphis*cpsis+q0*az*cphis*sthetas*spsis-q0*az*sphis*cpsis+2*q1*sthetas*ax-2*q1*sphis*cthetas*ay-2*q1*cphis*cthetas*az);	// dVD/dq1
	*(G+8+5*numStates) = 2*dt*(-q0*cthetas*cpsis*ax-q0*ay*sphis*sthetas*cpsis+q0*ay*cphis*spsis-q0*az*cphis*sthetas*cpsis-q0*az*sphis*spsis+q3*cthetas*spsis*ax+q3*ay*sphis*sthetas*spsis+q3*ay*cphis*cpsis+q3*az*cphis*sthetas*spsis-q3*az*sphis*cpsis+2*q2*sthetas*ax-2*q2*sphis*cthetas*ay-2*q2*cphis*cthetas*az);	// dVD/dq2
	*(G+9+5*numStates) = 2*dt*(q1*cthetas*cpsis*ax+q1*ay*sphis*sthetas*cpsis-q1*ay*cphis*spsis+q1*az*cphis*sthetas*cpsis+q1*az*sphis*spsis+q2*cthetas*spsis*ax+q2*ay*sphis*sthetas*spsis+q2*ay*cphis*cpsis+q2*az*cphis*sthetas*spsis-q2*az*sphis*cpsis);	// dVD/dq3
	*(G+17+5*numStates) = 2*dt*(-q1*q3*cthetas*spsis*ax-q1*q3*ay*sphis*sthetas*spsis-q1*q3*ay*cphis*cpsis-q1*q3*az*cphis*sthetas*spsis+q1*q3*az*sphis*cpsis+q0*q2*cthetas*spsis*ax+q0*q2*ay*sphis*sthetas*spsis+q0*q2*ay*cphis*cpsis+q0*q2*az*cphis*sthetas*spsis-q0*q2*az*sphis*cpsis+q2*q3*cthetas*cpsis*ax+q2*q3*ay*sphis*sthetas*cpsis-q2*q3*ay*cphis*spsis+q2*q3*az*cphis*sthetas*cpsis+q2*q3*az*sphis*spsis+q0*q1*cthetas*cpsis*ax+q0*q1*ay*sphis*sthetas*cpsis-q0*q1*ay*cphis*spsis+q0*q1*az*cphis*sthetas*cpsis+q0*q1*az*sphis*spsis);	// dVD/dpsis
	*(G+18+5*numStates) = dt*(-2*q1*q3*sthetas*cpsis*ax+2*q1*q3*sphis*cthetas*cpsis*ay+2*q1*q3*cphis*cthetas*cpsis*az+2*q0*q2*sthetas*cpsis*ax-2*q0*q2*sphis*cthetas*cpsis*ay-2*q0*q2*cphis*cthetas*cpsis*az-2*q2*q3*sthetas*spsis*ax+2*q2*q3*sphis*cthetas*spsis*ay+2*q2*q3*cphis*cthetas*spsis*az-2*q0*q1*sthetas*spsis*ax+2*q0*q1*sphis*cthetas*spsis*ay+2*q0*q1*cphis*cthetas*spsis*az-cthetas*ax-sphis*sthetas*ay-cphis*sthetas*az+2*q1*q1*cthetas*ax+2*q1*q1*sphis*sthetas*ay+2*q1*q1*cphis*sthetas*az+2*q2*q2*cthetas*ax+2*q2*q2*sphis*sthetas*ay+2*q2*q2*cphis*sthetas*az);	// dVD/dthetas
	*(G+19+5*numStates) = -dt*(-2*q1*q3*ay*cphis*sthetas*cpsis-2*q1*q3*ay*sphis*spsis+2*q1*q3*az*sphis*sthetas*cpsis-2*q1*q3*az*cphis*spsis+2*q0*q2*ay*cphis*sthetas*cpsis+2*q0*q2*ay*sphis*spsis-2*q0*q2*az*sphis*sthetas*cpsis+2*q0*q2*az*cphis*spsis-2*q2*q3*ay*cphis*sthetas*spsis+2*q2*q3*ay*sphis*cpsis+2*q2*q3*az*sphis*sthetas*spsis+2*q2*q3*az*cphis*cpsis-2*q0*q1*ay*cphis*sthetas*spsis+2*q0*q1*ay*sphis*cpsis+2*q0*q1*az*sphis*sthetas*spsis+2*q0*q1*az*cphis*cpsis-cphis*cthetas*ay+sphis*cthetas*az+2*q1*q1*cphis*cthetas*ay-2*q1*q1*sphis*cthetas*az+2*q2*q2*cphis*cthetas*ay-2*q2*q2*sphis*cthetas*az);	// dVD/dphis


	// Quaternion Jacobian elements (finite differencing!)
	double dx = 1e-6;
 	double *xp = (double*)malloc(sizeof(double)*numStates);	
 	double *xm = (double*)malloc(sizeof(double)*numStates);
	
	// Quaternion sensitivity to quaterions
	for (int j = 6; j < 10; j++) {
		
		// Perturb state
		for (int k = 0; k < numStates; k++) {
			*(xp+k) = *(x+k);
			*(xm+k) = *(x+k);
		}
		*(xp+j) += dx;
		*(xm+j) -= dx;
		
		// Compute new xp quaternion
		quaternionUpdate(xp,wx,wy,wz,dt);
		
		// Compute new xm quaternion
		quaternionUpdate(xm,wx,wy,wz,dt);
		
		// Finite difference
		*G(j+6*numStates) = (*(xp+6) - *(xm+6))/(2.0*dx);	// dq0/d..
		*G(j+7*numStates) = (*(xp+7) - *(xm+7))/(2.0*dx);	// dq1/d..
		*G(j+8*numStates) = (*(xp+8) - *(xm+8))/(2.0*dx);	// dq2/d..
		*G(j+9*numStates) = (*(xp+9) - *(xm+9))/(2.0*dx);	// dq3/d..
	}

	// Quaternion sensitivity to gyro biases
	for (int j = 12; j < 15; j++) {
		
		// Perturb state
		for (int k = 0; k < numStates; k++) {
			*(xp+k) = *(x+k);
			*(xm+k) = *(x+k);
		}
		*(xp+j) += dx;
		*(xm+j) -= dx;
		
		// Compute new xp quaternion
		quaternionUpdate(xp,wx,wy,wz,dt);
		
		// Compute new xm quaternion
		quaternionUpdate(xm,wx,wy,wz,dt);
		
		// Finite difference
		*G(j+6*numStates) = (*(xp+6) - *(xm+6))/(2.0*dx);	// dq0/d..
		*G(j+7*numStates) = (*(xp+7) - *(xm+7))/(2.0*dx);	// dq1/d..
		*G(j+8*numStates) = (*(xp+8) - *(xm+8))/(2.0*dx);	// dq2/d..
		*G(j+9*numStates) = (*(xp+9) - *(xm+9))/(2.0*dx);	// dq3/d..
	}
	
	// Quaternion sensitivity to sensor alignment angles
	for (int j = 17; j < 20; j++) {
		
		// Perturb state
		for (int k = 0; k < numStates; k++) {
			*(xp+k) = *(x+k);
			*(xm+k) = *(x+k);
		}
		*(xp+j) += dx;
		*(xm+j) -= dx;
		
		// Compute new xp quaternion
		quaternionUpdate(xp,wx,wy,wz,dt);
		
		// Compute new xm quaternion
		quaternionUpdate(xm,wx,wy,wz,dt);
		
		// Finite difference
		*G(j+6*numStates) = (*(xp+6) - *(xm+6))/(2.0*dx);	// dq0/d..
		*G(j+7*numStates) = (*(xp+7) - *(xm+7))/(2.0*dx);	// dq1/d..
		*G(j+8*numStates) = (*(xp+8) - *(xm+8))/(2.0*dx);	// dq2/d..
		*G(j+9*numStates) = (*(xp+9) - *(xm+9))/(2.0*dx);	// dq3/d..
	}
	
	// Parameter Jacobian elements
	for (int i = 10; i < numStates; i++) {
		*(G+i+i*numStates) = 1.0;
	}

 	free(xp); free(xm);
}



/**********************************
Compute just the quaternion part 
of the process model update
for finite differencing to get
process model Jacobian
**********************************/
void EKFKinematic::quaternionUpdate(double *x, double wx, double wy, double wz, double dt) {

	double q0 = *(x+6);
	double q1 = *(x+7);
	double q2 = *(x+8);
	double q3 = *(x+9);
	double wbx = *(x+12);
	double wby = *(x+13);
	double wbz = *(x+14);
	double psis = *(x+17);
	double thetas = *(x+18);
	double phis = *(x+19);

	double cpsis = cos(psis);
	double spsis = sin(psis);
	double cthetas = cos(thetas);
	double sthetas = sin(thetas);
	double cphis = cos(phis);
	double sphis = sin(phis);
	
	double Ts2b[9]; 	// Rotation matrix from sensor frame to body frame
	Ts2b[0] = cthetas*cpsis;
	Ts2b[1] = sphis*sthetas*cpsis-cphis*spsis;
	Ts2b[2] = cphis*sthetas*cpsis+sphis*spsis;
	Ts2b[3] = cthetas*spsis;
	Ts2b[4] = sphis*sthetas*spsis+cphis*cpsis;
	Ts2b[5] = cphis*sthetas*spsis-sphis*cpsis;
	Ts2b[6] = -sthetas;
	Ts2b[7] = sphis*cthetas;
	Ts2b[8] = cphis*cthetas;

	double dPhi   = (Ts2b[0]*(wx-wbx) + Ts2b[1]*(wy-wby) + Ts2b[2]*(wz-wbz))*dt;
	double dTheta = (Ts2b[3]*(wx-wbx) + Ts2b[4]*(wy-wby) + Ts2b[5]*(wz-wbz))*dt;
	double dPsi   = (Ts2b[6]*(wx-wbx) + Ts2b[7]*(wy-wby) + Ts2b[8]*(wz-wbz))*dt;
	double s = 0.5*sqrt(dPhi*dPhi + dTheta*dTheta + dPsi*dPsi);
	double coss = cos(s);
	double sins_over_s;
	if (s == 0) {
		sins_over_s = 1.0;
	} else {
		sins_over_s = sin(s)/s;
	}
	
	// Calculate new quaternions
	double qnew[4];
	qnew[0] = coss*q0 - 0.5*sins_over_s*(dPhi*q1 + dTheta*q2 + dPsi*q3);
	qnew[1] = coss*q1 - 0.5*sins_over_s*(-dPhi*q0 - dPsi*q2 + dtheta*q3);
	qnew[2] = coss*q2 - 0.5*sins_over_s*(-dTheta*q0 + dPsi*q1 - dPhi*q3);
	qnew[3] = coss*q3 - 0.5*sins_over_s*(-dPsi*q0 - dTheta*q1 + dPhi*q2);

	// Normalize quaternions and put back in input vector
	double invqmag = 1.0/sqrt(qnew[0]*qnew[0] + qnew[1]*qnew[1] + qnew[2]*qnew[2] + qnew[3]*qnew[3]);
	*(x+6) = qnew[0]*invqmag;
	*(x+7) = qnew[1]*invqmag;
	*(x+8) = qnew[2]*invqmag;
	*(x+9) = qnew[3]*invqmag;
}



/**********************************
Update convariance estimate based
on process model Jacobian

Should be...
Input: G,P,R,dt 
Output: Sbart
**********************************/
void EKFKinematic::computePredictedVariance(double *G, double *Sbart) {
	
	// Sbart = Gt*Stm1*Gt^T + Rt*dt
	double *tmp = (double*)malloc(sizeof(double)*numStates*numStates);
	double *Ptmp = (double*)malloc(sizeof(double)*numStates*numStates);

	int M = numStates;

	// Create local copy
	for (int i = 0; i < numStates*numStates; i++) {
		*(Ptmp+i) = *(P+i);
	}

	// Compute Gt*Stm1
	dgemm_("N","N",&M,&M,&M,&done,G,&M,Ptmp,&M,&dzero,tmp,&M);
	// Compute (Gt*Stm1)*Gt^T

	dgemm_("N","T",&M,&M,&M,&done,tmp,&M,G,&M,&dzero,Sbart,&M);
	// Add Rt
	for (int i = 0; i < numStates; i++) {
		*(Sbart+i+i*numStates) += R[i]*dt;
	}

	free(tmp); free(Ptmp);
}



/**********************************
Run measurement model - predict 
measurements at input state
**********************************/
void EKFKinematic::runMeasModel(double *x, double *zbt, bool flag) {

	// Extract state
	double Np = *(x+0);
	double Ep = *(x+1);
	double Dp = *(x+2);
	double VNi = *(x+3);
	double VEi = *(x+4);
	double VDi = *(x+5);
	double q0 = *(x+6);
	double q1 = *(x+7);
	double q2 = *(x+8);
	double q3 = *(x+9);
	double VNw = *(x+10);
	double VEw = *(x+11);
	double barob = *(x+15);
	double diffb = *(x+16);
	
	double Ti2b[9];		// Rotation matrix from inertial to body frame
	// In the form [ 0 1 2 ; 3 4 5 ; 6 7 8];
	Ti2b[0] = 1.0-2.0*q2*q2-2.0*q3*q3;
	Ti2b[1] = 2.0*q1*q2+2.0*q0*q3;
	Ti2b[2] = 2.0*q1*q3-2.0*q0*q2;
	Ti2b[3] = 2.0*q1*q2-2.0*q0*q3;
	Ti2b[4] = 1.0-2.0*q1*q1-2.0*q3*q3;
	Ti2b[5] = 2.0*q2*q3+2.0*q0*q1;
	Ti2b[6] = 2.0*q1*q3+2.0*q0*q2;
	Ti2b[7] = 2.0*q2*q3-2.0*q0*q1;
	Ti2b[8] = 1.0-2.0*q1*q1-2.0*q2*q2;
	
	*(zbt+0) = 101325.0*(pow(1.0+0.0000225721785*Dp,5.256)-barob;
	*(zbt+1) = 0.6125*((VNi-VNw)*(VNi-VNw)+(VEi-VEw)*(VEi-VEw)+VDi*VDi)-diffp;
	*(zbt+2) = Ti2b[0]*N_Mag_Field + Ti2b[1]*E_Mag_Field + Ti2b[2]*D_Mag_Field;
	*(zbt+3) = Ti2b[3]*N_Mag_Field + Ti2b[4]*E_Mag_Field + Ti2b[5]*D_Mag_Field;
	*(zbt+4) = Ti2b[6]*N_Mag_Field + Ti2b[7]*E_Mag_Field + Ti2b[8]*D_Mag_Field;
	
	if (!flag) { // GPS update too
		*(zbt+5) = Np;
		*(zbt+6) = Ep;
		*(zbt+7) = Dp;
		*(zbt+8) = VNi;
		*(zbt+9) = VEi;
	}
}



/**********************************
Compute Jacobian of measurement model
**********************************/
void EKFKinematic::computeMeasJacobian(double *x, double *H, bool flag) {

	// i = function (N)
	// j = state (numstates)
	
	int N;
	// H(i,j) = *(H+i+j*N)
	if (flag) {
		N = imuSize;
	} else {
		N = imuSize+gpsSize;
	}
	
	// Initialize Jacobian to zeros
	for (int i = 0; i < N; i++) {
		for (int j = 0; j < numStates; j++) {
			*(H+i+j*N) = 0.0;
		}
	}	

	// Extract state
	double Dp = *(x+2);
	double VNi = *(x+3);
	double VEi = *(x+4);
	double VDi = *(x+5);
	double q0 = *(x+6);
	double q1 = *(x+7);
	double q2 = *(x+8);
	double q3 = *(x+9);
	double VNw = *(x+10);
	double VEw = *(x+11);
	
	// Baro pressure Jacobian elements
	*(H+0+2*N) = 12.021134185109700*pow(1+0.0000225721785*Dp,4.256); 	// dbb/dD
	*(H+0+15*N) = -1.0;	// dbb/dbb

	// Differential pressure Jacobian elements
	*(H+1+3*N) = 1.225*(VNi-VNw);	// ddb/dVN
	*(H+1+4*N) = 1.225*(VEi-VEw);	// ddb/dVE
	*(H+1+5*N) = 1.225*VDi;			// ddb/dVD
	*(H+1+10*N) = -1.225*(VNi-VNw);	// ddb/dVWN
	*(H+1+11*N) = -1.225*(VEi-VEw);	// ddb/dVWE
	*(H+1+16*N) = -1.0;				// ddb/ddb

	// Magnetometer Jacobian elements
	*(H+2+6*N) = 2*q3*E_Mag_Field - 2*q2*D_Mag_Field;						// dMX/dq0
	*(H+2+7*N) = 2*q2*E_Mag_Field + 2*q3*D_Mag_Field;						// dMX/dq1
	*(H+2+8*N) = -4*q2*N_Mag_Field + 2*q1*E_Mag_Field - 2*q0*D_Mag_Field;	// dMX/dq2
	*(H+2+9*N) = -4*q3*N_Mag_Field + 2*q0*E_Mag_Field + 2*q1*D_Mag_Field;	// dMX/dq3

	*(H+3+6*N) = -2*q3*N_Mag_Field + 2*q1*D_Mag_Field;						// dMY/dq0
	*(H+3+7*N) = 2*q2*N_Mag_Field - 4*q1*E_Mag_Field + 2*q0*D_Mag_Field;	// dMY/dq1
	*(H+3+8*N) = 2*q1*N_Mag_Field + 2*q3*D_Mag_Field;						// dMY/dq2
	*(H+3+9*N) = -2*q0*N_Mag_Field - 4*q3*E_Mag_Field + 2*q2*D_Mag_Field;	// dMY/dq3

	*(H+4+6*N) = 2*q2*N_Mag_Field - 2*q1*E_Mag_Field;						// dMZ/dq0
	*(H+4+7*N) = 2*q3*N_Mag_Field - 2*q0*E_Mag_Field - 4*q1*D_Mag_Field;	// dMZ/dq1
	*(H+4+8*N) = 2*q0*N_Mag_Field + 2*q3*E_Mag_Field - 4*q2*D_Mag_Field;	// dMZ/dq2
	*(H+4+9*N) = 2*q1*N_Mag_Field + 2*q2*E_Mag_Field;						// dMZ/dq3

	// GPS Jacobian elements
	if (!flag) {
		*(H+5+0*N) = 1.0;
		*(H+6+1*N) = 1.0;
		*(H+7+2*N) = 1.0;
		*(H+8+3*N) = 1.0;
		*(H+9+4*N) = 1.0;
	}
}




/**********************************
Compute the Kalman gain
**********************************/
void EKFKinematic::computeKalmanGain(double *Sbart, double *H, double *Kt, bool flag) {

	int N, M, info;
	N = numStates;

	if (flag) { // IMU
		M = imuSize;
	} else { // GPS+IMU
		M = gpsSize + imuSize;
	}

	int *ipiv = (int*)malloc(sizeof(double)*numStates);
	double *tmp = (double*)malloc(sizeof(double)*numStates*M);
	double *tmp2 = (double*)malloc(sizeof(double)*numStates*M);
	double *tmp3 = (double*)malloc(sizeof(double)*numStates*numStates);
	double *Ptmp = (double*)malloc(sizeof(double)*numStates*numStates);
	double *t1  = (double*)malloc(sizeof(double)*numStates*numStates);
	double *t2  = (double*)malloc(sizeof(double)*numStates*numStates);
	double *Ht  = (double*)malloc(sizeof(double)*numStates*M);

	// Put Ht into row major format for FORTRAN
	for (int i = 0; i < numStates; i++) {
		for (int j = 0; j < M; j++) {
			*(Ht+j+i*M) = *(H+i+j*numStates);
		}
	}

	// Kt = Sbart*Ht^T*(Ht*Sbart*Ht^T+Qt)^-1

	// Compute Ht*Sbart
	dgemm_("N","N",&M,&N,&N,&done,Ht,&M,Sbart,&N,&dzero,tmp,&M);

	// Compute (Ht*Sbart)*Ht^T
	dgemm_("N","T",&M,&M,&N,&done,tmp,&M,Ht,&M,&dzero,tmp2,&M);
	for (int i = 0; i < numStates; i++) {
		if (flag) {
			*(tmp2+i) += Q_IMU[i];
		} else {
			*(tmp2+i) += Q_IMU_GPS[i];
		}
	}

	// Create local copy
	for (int i = 0; i < numStates*numStates; i++) {
		*(Ptmp+i) = *(P+i);
	}

	// Compute Sbart*Ht^T
	dgemm_("N","T",&N,&M,&N,&done,Sbart,&N,Ht,&M,&dzero,tmp3,&N);

	// Compute transpose of (Sbart*Ht^T) and (Ht*Sbart*Ht^T)+Qt
	for (int i = 0; i < numStates; i++) {
		for (int j = 0; j < M; j++) {
			*(t1+j+i*M) = *(tmp3+i+j*numStates); // (Sbart*Ht^T)^T
		}
	}

	for (int i = 0; i < M; i++) {
		for (int j = 0; j < M; j++) {
			*(t2+i+j*M) = *(tmp2+j+i*M);
		}
	}

	// Matrix solve
	dgesv_(&M, &N, t2, &M, ipiv, t1, &M, &info);

	for (int i = 0; i < M; i++) {
		for (int j = 0; j < N; j++) {
			*(Kt+i+j*M) = *(t1+i+j*M);
		}
	}

	free(tmp); free(tmp2); free(tmp3); free(Ptmp); free(t1); free(t2); free(ipiv);
}



/**********************************
Update state and covariance estimates
**********************************/
void EKFKinematic::computeNewMeanandCovariance(double *mubt, double *Kt, double *zbt,
		double *Sbart, double *Ht, bool flag) {

	int N, M;
	N = numStates;
	double dzero = 0.0;
	double dmone = -1.0;

	if (flag) { // IMU
		M = imuSize;
	} else { // GPS+IMU
		M = gpsSize + imuSize;
	}

	double *Ktz = (double*)malloc(sizeof(double)*N);
	double *zt  = (double*)malloc(sizeof(double)*M);
	double *mut = (double*)malloc(sizeof(double)*N);
	double *tmp = (double*)malloc(sizeof(double)*M);
	double *eyeN = (double*)malloc(sizeof(double)*N*N);

	// Pressure and magnetometer measurements
	*(zt+0) = apdat.baropress;
	*(zt+1) = apdat.diffpress;
	*(zt+2) = apdat.mx;
	*(zt+3) = apdat.my;
	*(zt+4) = apdat.mz;
	if (!flag) {	// GPS measurements too
		*(zt+5) = apdat.GPS_N;
		*(zt+6) = apdat.GPS_E;
		*(zt+7) = apdat.GPS_D;
		*(zt+8) = apdat.GPS_VN;
		*(zt+9) = apdat.GPS_VE;
	} 


	// Write identity matrix
	for (int i = 0; i < numStates; i++) {
		for (int j = 0; j < numStates; j++) {
			*(eyeN+i+j*N) = 0.0;
			if (i == j) {
				*(eyeN+i+j*N) = 1.0;
			}
		}
	}

	// Real measurement minus predicted measurement
	for (int i = 0; i < M; i++) {
		*(tmp+i) = (*(zt+i)-*(zbt+i));
	}

//	// Kt*(zt-zbt)
	dgemv_("N", &N, &M, &done, Kt, &N, tmp, &ione, &dzero, Ktz, &ione);

	// Compute new estimate
	for (int i = 0; i < numStates; i++) {
		*(mut+i) = *(mubt+i) + *(Ktz+i);
		cout << *(mut+i) << endl;
	}

	// Put new state estimate into the global variables
	updateState(mut);
//	cout << *(mut+0) << " " << *(mut+1) << " " << *(mut+2) << endl;

	// (I-Kt*Ht)
	dgemm_("N","N", &N, &N, &M, &dmone, Kt, &N, Ht, &M, &done, eyeN, &N);

	// (I-Kt*Ht)*Sbart
//	dgemm_("N","N", &N, &N, &N, &done, eyeN, &N, Sbart, &N, &dzero, P, &N);

	free(Ktz); free(zt); free(mut); free(tmp); free(eyeN);
}




/**********************************
Save state to global variable
**********************************/
void EKFKinematic::updateState(double *mubt) {
	if (*(mubt+0) > 1e6) {
		sv->Np = 1e6;
	} else if (*(mubt+0) < -1e6) {
		sv->Np = -1e6;
	} else {
		sv->Np = *(mubt+0);
	}
	if (*(mubt+1) > 1e6) {
		sv->Ep = 1e6;
	} else if (*(mubt+1) < -1e6) {
		sv->Ep = -1e6;
	} else {
		sv->Ep = *(mubt+1);
	}
	if (*(mubt+2) > 1e6) {
		sv->Dp = 1e6;
	} else if (*(mubt+2) < -1e6) {
		sv->Dp = -1e6;
	} else {
		sv->Dp = *(mubt+2);
	}
	if (*(mubt+3) > 50) {
		sv->VNi = 50;
	} else if (*(mubt+3) < -50) {
		sv->VNi = -50;
	} else {
		sv->VNi = *(mubt+3);
	}
	if (*(mubt+4) > 50) {
		sv->VEi = 50;
	} else if (*(mubt+4) < -50) {
		sv->VEi = -50;
	} else {
		sv->VEi = *(mubt+4);
	}
	if (*(mubt+5) > 50) {
		sv->VDi = 50;
	} else if (*(mubt+5) < -50) {
		sv->VDi = -50;
	} else {
		sv->VDi = *(mubt+5);
	}
	if (*(mubt+6) > 1) {
		sv->q0 = 1;
	} else if (*(mubt+6) < -1) {
		sv->q0 = -1;
	} else {
		sv->q0 = *(mubt+6);
	}
	if (*(mubt+7) > 1) {
		sv->q1 = 1;
	} else if (*(mubt+7) < -1) {
		sv->q1 = -1;
	} else {
		sv->q1 = *(mubt+7);
	}
	if (*(mubt+8) > 1) {
		sv->q2 = 1;
	} else if (*(mubt+8) < -1) {
		sv->q2 = -1;
	} else {
		sv->q2 = *(mubt+8);
	}
	if (*(mubt+9) > 1) {
		sv->q3 = 1;
	} else if (*(mubt+9) < -1) {
		sv->q3 = -1;
	} else {
		sv->q3 = *(mubt+9);
	}
	if (*(mubt+10) > 20) {
		sv->p = 20;
	} else if (*(mubt+10) < -20) {
		sv->p = -20;
	} else {
		sv->p = *(mubt+10);
	}
	if (*(mubt+11) > 20) {
		sv->q = 20;
	} else if (*(mubt+11) < -20) {
		sv->q = -20;
	} else {
		sv->q = *(mubt+11);
	}
	if (*(mubt+12) > 20) {
		sv->r = 20;
	} else if (*(mubt+12) < -20) {
		sv->r = -20;
	} else {
		sv->r = *(mubt+12);
	}
	if (*(mubt+13) > 50) {
		pv->VNi_wind = 50;
	} else if (*(mubt+13) < -50) {
		pv->VNi_wind = -50;
	} else {
		pv->VNi_wind = *(mubt+13);
	}
	if (*(mubt+14) > 50) {
		pv->VEi_wind = 50;
	} else if (*(mubt+14) < -50) {
		pv->VEi_wind = -50;
	} else {
		pv->VEi_wind = *(mubt+14);
	}
	if (*(mubt+15) > 50) {
		pv->VDi_wind = 50;
	} else if (*(mubt+15) < -50) {
		pv->VDi_wind = -50;
	} else {
		pv->VDi_wind = *(mubt+15);
	}
	if (*(mubt+16) > 10) {
		pv->abx = 10;
	} else if (*(mubt+16) < -10) {
		pv->abx = -10;
	} else {
		pv->abx = *(mubt+16);
	}
	if (*(mubt+17) > 10) {
		pv->aby = 10;
	} else if (*(mubt+17) < -10) {
		pv->aby = -10;
	} else {
		pv->aby = *(mubt+17);
	}
	if (*(mubt+18) > 10) {
		pv->abz = 10;
	} else if (*(mubt+18) < -10) {
		pv->abz = -10;
	} else {
		pv->abz = *(mubt+18);
	}
	if (*(mubt+19) > 10) {
		pv->wbx = 10;
	} else if (*(mubt+19) < -10) {
		pv->wbx = -10;
	} else {
		pv->wbx = *(mubt+19);
	}
	if (*(mubt+20) > 10) {
		pv->wby = 10;
	} else if (*(mubt+20) < -10) {
		pv->wby = -10;
	} else {
		pv->wby = *(mubt+20);
	}
	if (*(mubt+21) > 10) {
		pv->wbz = 10;
	} else if (*(mubt+21) < -10) {
		pv->wbz = -10;
	} else {
		pv->wbz = *(mubt+21);
	}
	pv->barob = *(mubt+22);
	pv->diffb = *(mubt+23);
}




/**********************************
Write data to log file
**********************************/
void EKFKinematic::printData(double t) {
	if (t > 600+ts) {
		Sfile->close();
	}
	if (Sfile->is_open()) {
		*Sfile << t << " " << sv->Np << " "  <<  sv->Ep  << " " <<  sv->Dp << " " <<
			      sv->VNi << " " <<  sv->VEi << " " <<  sv->VDi << " " <<
			      sv->q0  << " " <<  sv->q1 << " " <<   sv->q2 << " " <<   sv->q3 << " " <<
			      sv->p << " "   <<   sv->q << " " <<   sv->r << " " <<
			      pv->VNi_wind << " " <<  pv->VEi_wind << " " <<  pv->VDi_wind << " " <<
			      pv->abx << " " << pv->aby << " " << pv->abz << " " <<
			      pv->wbx << " " << pv->wby << " " << pv->wbz << " " <<
			      pv->barob << " " << pv->diffb << " " << apdat.GPS_N << " " <<
			      apdat.GPS_E << " " << apdat.GPS_D << " " << apdat.GPS_VN << " " <<
			      apdat.GPS_VE << " " << apdat.baropress << " " << apdat.diffpress << " " <<
			      apdat.ax1 << " " << apdat.ay1 << " " << apdat.az1 << " " <<
			      apdat.ax2 << " " << apdat.ay2 << " " << apdat.az2 << " " <<
			      apdat.mx << " " << apdat.my << " " << apdat.mz << " " <<
			      apdat.p << " " << apdat.q << " " << apdat.r << " " <<
			      apdat.barotemp << " " << apdat.GPStime << " " <<
			      cv.ailDefRad << " " << cv.eleDefRad << " " << cv.rudDefRad << " " <<
			      cv.thrPercent << endl;
	}
}




/**********************************
Close log file
**********************************/
void EKFKinematic::closeFile() {
	Sfile->close();
}


/**********************************
Destructor - do not change
**********************************/
EKFKinematic::~EKFKinematic() {
}
